package model.utils;

import javax.vecmath.Matrix3d;
import javax.vecmath.Point3d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;

public class Putils {

	public static Point3d scalex(Point3d p, double s) {
		return new Point3d(p.x*s,p.y,p.z);
	}
	public static Point3d scaley(Point3d p, double s) {
		return new Point3d(p.x,p.y*s,p.z);
	}
	public static Point3d scalez(Point3d p, double s) {
		return new Point3d(p.x,p.y,p.z*s);
	}
	
	public static Point3d[] rotatez(Point3d[] v, double d){
		Matrix3d m = new Matrix3d(  cosDeg(d),-sinDeg(d),0,
									sinDeg(d),cosDeg(d),0,
									0,0,1);
		return rotate(m,v);
	}

	public static Point3d[] rotatey(Point3d[] v, double d){
		Matrix3d m = new Matrix3d(  cosDeg(d),0,sinDeg(d),
									0,1,0,
									-sinDeg(d),0,cosDeg(d));
		return rotate(m,v); 
	}

	public static Point3d[] rotatex(Point3d[] v, double d){
		Matrix3d m = new Matrix3d(  1,0,0,
									0,cosDeg(d),-sinDeg(d),
									0,sinDeg(d),cosDeg(d));
		return rotate(m,v); 
	}
	
	public static Vector3d[] rotatez(Vector3d[] v, double d){
		Matrix3d m = new Matrix3d(  cosDeg(d),-sinDeg(d),0,
				sinDeg(d),cosDeg(d),0,
				0,0,1);
		return rotate(m,v);
	}
	
	public static Vector3d[] rotatey(Vector3d[] v, double d){
		Matrix3d m = new Matrix3d(  cosDeg(d),0,sinDeg(d),
				0,1,0,
				-sinDeg(d),0,cosDeg(d));
		return rotate(m,v); 
	}
	
	public static Vector3d[] rotatex(Vector3d[] v, double d){
		Matrix3d m = new Matrix3d(  1,0,0,
				0,cosDeg(d),-sinDeg(d),
				0,sinDeg(d),cosDeg(d));
		return rotate(m,v); 
	}
	
	private static Point3d[] rotate(Matrix3d m, Point3d[] v){
		Point3d[] nv=new Point3d[3];
		nv[0]=mult(v[0],m);
		nv[1]=mult(v[1],m);
		nv[2]=mult(v[2],m);
		return nv; 
	}
	
	private static Point3d mult(Tuple3d p, Matrix3d m){
		
		return new Point3d(	p.x*m.m00+p.y*m.m10+p.z*m.m20, 
							p.x*m.m01+p.y*m.m11+p.z*m.m21, 
							p.x*m.m02+p.y*m.m12+p.z*m.m22);
	}
	
	
	private static Vector3d[] rotate(Matrix3d m, Vector3d[] v){
		Vector3d[] nv=new Vector3d[3];
		nv[0]=mult(v[0],m);
		nv[1]=mult(v[1],m);
		nv[2]=mult(v[2],m);
		return nv; 
	}
	
	private static Vector3d mult(Vector3d p, Matrix3d m){
		
		return new Vector3d(	p.x*m.m00+p.y*m.m10+p.z*m.m20, 
				p.x*m.m01+p.y*m.m11+p.z*m.m21, 
				p.x*m.m02+p.y*m.m12+p.z*m.m22);
	}

	
	private static double cosDeg(double d){
		return Math.cos(-d*Math.PI/180f);	
	}
	
	private static double sinDeg(double d){
		return Math.sin(-d*Math.PI/180f);	
	}
}
